#pragma once
#include "Vector.h"
#include "Matrix.h"

#define PIOVER180 0.017453292519943295769236907684886f
#define EPSILON 0.00001f

class CQuaternion
{
public:
	float x, y, z, w;
	CQuaternion():x(0), y(0), z(0), w(1){};
	CQuaternion(CVector3 &v, float w):x(v.x), y(v.y), z(v.z), w(w){};
	CQuaternion(float qx, float qy, float qz, float qw):x(qx), y(qy), z(qz), w(qw){};
	CQuaternion &normalize()
	{
		float mag2 = w * w + x * x + y * y + z * z;
		if (fabs(mag2) > EPSILON)
		{
			float mag = sqrt(mag2);
			w /= mag;
			x /= mag;
			y /= mag;
			z /= mag;
		}
		return *this;
	};

	CQuaternion getConjugate() const {return CQuaternion(-x, -y, -z, w);};
	CQuaternion& operator= (const CQuaternion& q)
	{
		w = q.w;
		x = q.x;
		y = q.y;
		z = q.z;
		return *this;
	}
	CQuaternion operator* (const CQuaternion &q) const
	{
		return CQuaternion(
			w * q.x + x * q.w + y * q.z - z * q.y,
			w * q.y - x * q.z + y * q.w + z * q.x,
			w * q.z + x * q.y - y * q.x + z * q.w,
			w * q.w - x * q.x - y * q.y - z * q.z
			);
	};
	CVector3 operator* (const CVector3 &vec) const
	{
		CVector3 vn(vec);
		vn.normalize();
		CQuaternion vecQuat(vn.x, vn.y, vn.z, 0.0f);
		/*
		vecQuat.x = vn.x;
		vecQuat.y = vn.y;
		vecQuat.z = vn.z;
		vecQuat.w = 0.0f;
		*/
		CQuaternion &resQuat = vecQuat * getConjugate();
		resQuat = *this * resQuat;

		return (CVector3(resQuat.x, resQuat.y, resQuat.z));
	};

	// rotate through an angle theta, about the axis v (unit vector)
	void FromAxis(CVector3& v, float theta)
	{
		float sinAngle;
		theta *= 0.5f;
		v.normalize();

		sinAngle = sin(theta);

		x = (v.x * sinAngle);
		y = (v.y * sinAngle);
		z = (v.z * sinAngle);
		w = cos(theta);
	};

	// Convert from Euler Angles
	void FromEuler(float pitch, float yaw, float roll)
	{
		// Basically we create 3 Quaternions, one for pitch, one for yaw, one for roll
		// and multiply them together.
		// the calculation below does the same, just shorter

		float e_p = pitch * PIOVER180 / 2.0f;
		float e_y = yaw * PIOVER180 / 2.0f;
		float e_r = roll * PIOVER180 / 2.0f;

		float sinp = sin(e_p);
		float siny = sin(e_y);
		float sinr = sin(e_r);
		float cosp = cos(e_p);
		float cosy = cos(e_y);
		float cosr = cos(e_r);

		x = sinr * cosp * cosy - cosr * sinp * siny;
		y = cosr * sinp * cosy + sinr * cosp * siny;
		z = cosr * cosp * siny - sinr * sinp * cosy;
		w = cosr * cosp * cosy + sinr * sinp * siny;

		normalize();
	};

	// Convert to Matrix
	CMatrix4 getMatrix() const
	{
		float x2 = x * x;
		float y2 = y * y;
		float z2 = z * z;
		float xy = x * y;
		float xz = x * z;
		float yz = y * z;
		float wx = w * x;
		float wy = w * y;
		float wz = w * z;

		// Unit quaternion property w2 + x2 + y2 + z2 = 1 is used to reduce the matrix expression
		// (see expressions of diagonal terms)
		// Note: The constructor of Matrix4 expects the Matrix in column-major format like expected by
		// OpenGL
		return CMatrix4(
			1.0f - 2.0f * (y2 + z2),	2.0f * (xy - wz),			2.0f * (xz + wy),			0.0f,
			2.0f * (xy + wz),			1.0f - 2.0f * (x2 + z2),	2.0f * (yz - wx),			0.0f,
			2.0f * (xz - wy),			2.0f * (yz + wx),			1.0f - 2.0f * (x2 + y2),	0.0f,
			0.0f,						0.0f,						0.0f,						1.0f);
	};

	/* replaces a naive call to
		CMatrix& m = _orientation.getMatrix().transpose();
		_posistion = CVector(-m.z1, -m.z2, -m.z3);
	*/
	CVector3 getDirectionVector() const
	{
		return CVector3(
					-2.0f * (x * z - w * y),
					-2.0f * (y * z + w * x),
			-(1.0f - 2.0f * (x * x + y * y)
			));
	}

	// Convert to Axis/Angles
	void getAxisAngle(CVector3 &axis, float &angle) const
	{
//		normalize();
		float scale = sqrt(x * x + y * y + z * z);
		if (scale < EPSILON)
		{
			axis.x = 1.0f;
			axis.y = 0.0f;
			axis.z = 0.0f;
			angle = 0.0f;
		}else{
			axis.x = x / scale;
			axis.y = y / scale;
			axis.z = z / scale;
			angle = acos(w) * 2.0f;
			axis.normalize();
		}
	}

};
